/* 
    CircleTracker.cpp
	Author: Mike Ciaraldi

	Returns values simulating an object moving
	in a circle of fixed radius at a constant distance.

*/

#include <iostream>
#include <cstring>
#include <string>
#include <math.h>
#include <stdio.h>

#include "CircleTracker.h"

#ifndef _WINDOWS
#define sprintf_s snprintf
#endif


using namespace std;

// Constructor(s) and Destructor
CircleTracker::CircleTracker(void) {
  // set defaults
  center_x = DEFAULT_CIRCLE_CENTER_X;
  center_y = DEFAULT_CIRCLE_CENTER_Y;
  center_z = DEFAULT_CIRCLE_CENTER_Z;
  radius = DEFAULT_CIRCLE_RADIUS;
  current_x = center_x;
  current_y = center_y + radius;
  current_z = 0;
  angle_increment = DEFAULT_ANGLE_INCREMENT;
  current_angle = 0.0;
};
 
CircleTracker::~CircleTracker(void) {};
	
 // Methods

int CircleTracker::open() { return true; }

int CircleTracker::close()  { return true; }

int CircleTracker::poll(float *out) {
  move(); // Move to next position
  out[0] = (float)current_x;
  out[1] = (float)current_y;
  out[2] = (float)current_z;
  out[3] = 0.0;
  out[4] = 0.0;
  out[5] = 0.0;

  return true; 
};

void CircleTracker::move() {
  current_angle += angle_increment;
  current_y = center_y + radius * cos(current_angle);
  current_z = center_z + radius * sin(current_angle);
}




int CircleTracker::start(){ 
	return true; 
};

int CircleTracker::stop(){ 
	return true; 
};

int CircleTracker::read(){ 
	return true; 
};

int CircleTracker::nextFrame(){
  move(); 
  return true;
};

int CircleTracker::getData(int sensor, int channel, float &value){
	switch(channel){
	case 0:
		value = (float) current_x;
		break;
	case 1:
		value = (float) current_y;
		break;
	case 2:
		value = (float) current_z;
		break;
	case 3:
	case 4:
	case 5:
		value = 0.0;
		break;
	default:
		sprintf_s(errorMsg,CIRCLE_TRACKER_ERROR_MSG_SIZE,
			"CircleTracker::getData - Out of range channel\n");
		return false;
	}
	return true; 
};

char *CircleTracker::getErrorInfo(void){
	return errorMsg;
};	

int CircleTracker::sync(void) {
  return true;
};
